#include "modbus.h"
#include "usart.h"
#include "string.h"

int modbusTimeoutCount=0;
int modbusRxFlag=0;

uint8_t modbusRxBuf[MODBUS_RX_BUF_SIZE];
int modbusRxLen=0;
uint8_t modbusTxBuf[MODBUS_TX_BUF_SIZE];
int modbusTxLen=0;
uint16_t modbusRegister[MODBUS_REG_SIZE]={0};

uint16_t crcRecv;
uint16_t crcCalc;
	
int frameCount=0;

int mbus_03_rx_count=0;

uint16_t mbus_03_request_addr=0;

const uint16_t polynom = 0xA001;
	
uint16_t crc16(uint8_t *ptr, uint16_t len)
{
    uint8_t i;
    uint16_t crc = 0xffff;

	if(len>1024)
	{
		return 0;
	}
	
    if (len == 0) 
	{
        len = 1;
    }
    while (len--) 
	{
        crc ^= *ptr;
        for (i = 0; i<8; i++)
        {
            if (crc & 1) 
            {
                crc >>= 1;
                crc ^= polynom;
            }
            else 
            {
                crc >>= 1;
            }
        }
        ptr++;
    }
    
    return(crc);
}

void modbus_send_buf(uint8_t * buf, int len)
{
	HAL_HalfDuplex_EnableTransmitter(&huart1);
	
	uart_send_buf(buf,len);
	
	HAL_Delay(2); //if remove, will communication fail
	
	HAL_HalfDuplex_EnableReceiver(&huart1);
}

void modbus_03_request(uint16_t addr, uint16_t num)
{
    uint16_t crc;
    mbus_03_request_addr = addr;
    
    modbusTxBuf[0] = 0x01;
    modbusTxBuf[1] = 0x03;
    modbusTxBuf[2] = addr>>8;
    modbusTxBuf[3] = addr;
    modbusTxBuf[4] = num>>8;
    modbusTxBuf[5] = num;
    
    crc = crc16(modbusTxBuf,6);
	modbusTxBuf[6] = crc;
	modbusTxBuf[7] = crc>>8;
    
    modbus_send_buf(modbusTxBuf,8);
}

void modbus_03_response(uint8_t * buf, int len)
{
	int dataLen;
	int i;
		
    if(mbus_03_request_addr==0)
    {
        return;
    }
    
	dataLen = modbusRxBuf[2] / 2;
	
	for(i=0;i<dataLen;i++)
	{
        modbusRegister[mbus_03_request_addr+i] = (modbusRxBuf[3 + i * 2]<<8) | modbusRxBuf[3 + i * 2 + 1];
	}
    
    mbus_03_request_addr = 0;
}


void modbus_poll(void)
{	
	int i=0;
	uint8_t cmd;
	(void)i;
	
	if(modbusRxFlag)
	{
		crcRecv = modbusRxBuf[modbusRxLen-2] | modbusRxBuf[modbusRxLen-1]<<8;
		crcCalc = crc16(modbusRxBuf,modbusRxLen-2);
		if(crcCalc == crcRecv)
		{
			frameCount++;
			
			modbusTxBuf[0] = 1;
			cmd = modbusRxBuf[1];
			if(modbusRxBuf[0]==1)
			{
				switch(cmd)
				{
					case 0x03:
						modbus_03_response(modbusRxBuf,modbusRxLen);
                        mbus_03_rx_count++;
						break;
					default:
						break;
				}
			}
		}
		
		memset(modbusRxBuf,0,MODBUS_RX_BUF_SIZE);
		modbusRxLen = 0;
		modbusRxFlag = 0;
		modbusRxLen = 0;
	}
}

void modbus_rx_callback(uint8_t data)
{
	modbusRxBuf[modbusRxLen] = data;
	modbusRxLen++;
	if(modbusRxLen >= MODBUS_RX_BUF_SIZE)
	{
		modbusRxLen = 0;
	}
		
	modbusTimeoutCount = 0;
}


void modbus_timer_callback(void)
{
	if(modbusRxLen>0)
	{
		modbusTimeoutCount++;
		if(modbusTimeoutCount>10)
		{
			modbusRxFlag = 1;			
		}
	}
}



